/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/
#ifndef TROTTING_H
#define TROTTING_H

#include "FSM/FSMState.h"
#include "Gait/GaitGenerator.h"
#include "control/BalanceCtrl.h"

class State_Trotting : public FSMState{
public:
    State_Trotting(CtrlComponents *ctrlComp);
    ~State_Trotting();
    void enter();
    void run();
    void exit();
    virtual FSMStateName checkChange();
    void setHighCmd(double vx, double vy, double wz);
private:
    /// @brief 由期望物理量计算关节力矩
    void calcTau();
    void calcQQd();
    /// @brief 根据期望机身(角)速度,计算期望机身位置 & yaw角
    void calcCmd();
    /// @brief 处理用户指令, 得到期望机身(角)速度
    virtual void getUserCmd();
    void calcBalanceKp();
    bool checkStepOrNot();

    GaitGenerator *_gait;
    Estimator *_est;    //状态估计器, 用于评估{s}下物理量
    QuadrupedRobot *_robModel;
    BalanceCtrl *_balCtrl;

    // Rob State
    Vec3  _posBody, _velBody;   //state: {s}下机身位置/速度
    double _yaw, _dYaw;     //state: {s}下机身yaw角(速)度
    Vec34 _posFeetGlobal, _velFeetGlobal;   //state: 足端在{s}下位置(速度)
    Vec34 _posFeet2BGlobal;     //state: 足端在{s}下相对于{b}中心位置
    RotMat _B2G_RotMat, _G2B_RotMat;    //state: Rsb(B2G)/Rsb^T(G2B)
    Vec12 _q;   //state: 关节角度

    // Robot command
    Vec3 _pcd;  //机身在{s}下目标位置
    Vec3 _vCmdGlobal, _vCmdBody;    //机身在{frame}下目标速度
    double _yawCmd, _dYawCmd;       //机身在{s}下目标yaw角(速度)
    double _dYawCmdPast;
    Vec3 _wCmdGlobal;      //机身在{s}下目标旋转角速度向量
    Vec34 _posFeetGlobalGoal, _velFeetGlobalGoal;   //足端在{s}下目标位置(速度)
    Vec34 _posFeet2BGoal, _velFeet2BGoal;       //足端在{b}下目标位置(速度)
    RotMat _Rd;     //机身目标姿态旋转矩阵
    Vec3 _ddPcd, _dWbd;     //机身在{s}下目标(角)加速度
    Vec34 _forceFeetGlobal, _forceFeetBody;     //{s}/{b}下目标足端力
    Vec34 _qGoal, _qdGoal;  //关节目标角(速)度
    Vec12 _tau;     //关节前馈力矩

    // Control Parameters
    double _gaitHeight;     //摆动退抬腿高度
    Vec3 _posError, _velError;     //{s}下机身位置(速度)误差
    Mat3 _Kpp, _Kdp, _Kdw;      //机身位姿反馈控制增益
    double _kpw;        //机身位姿反馈控制增益
    Mat3 _KpSwing, _KdSwing;        //摆动退修正力刚度系数
    Vec2 _vxLim, _vyLim, _wyawLim;      //{b}下限制区间
    Vec4 *_phase;       //相位?
    VecInt4 *_contact;      

    // Calculate average value
    AvgCov *_avg_posError = new AvgCov(3, "_posError", true, 1000, 1000, 1);
    AvgCov *_avg_angError = new AvgCov(3, "_angError", true, 1000, 1000, 1000);
};

#endif  // TROTTING_H